
#ifndef MSE_MOTOR_H
#define MSE_MOTOR_H

#include "stdint.h"
#include "MSE_MATH.h"
#include "string.h"
#include "MY_define.h"

struct MSE_MOTOR_DATA_Typedef
{
    int8_t ONLINE_JUDGE_TIME;
    int16_t Angle_last; // 上一个角度值
    int16_t Angle_now;  // 现在的角度值
    int16_t Speed_last; // 上一个速度值
    int16_t Speed_now;  // 现在的速度值
    int16_t current;
    int8_t temperature;
    int32_t Angle_Infinite;
    int32_t Stuck_Time;
	uint16_t Stuck_Flag[2];
    int16_t Laps;
    float Error;
    float Aim;
    float Aim_last;
    float dt;
    uint32_t DWT_CNT;
	float pos;
	float vel;
	 uint8_t is_stuck;  
};



struct MSE_MOTOR_PID_Typedef
{
    float P;
    float I;
    float D;
    int32_t I_Lit;
    int32_t All_Lit;

    float Error[2];
    float P_out;
    float I_out;
    int32_t D_out;
    float All_out;
};

struct MSE_MOTOR_Typedef
{
    uint8_t PID_INIT;
    struct MSE_MOTOR_DATA_Typedef DATA;
    struct MSE_MOTOR_PID_Typedef PID_P;
    struct MSE_MOTOR_PID_Typedef PID_S;
    struct MSE_MOTOR_PID_Typedef PID_C;  //电流环
};

//升降
extern struct MSE_MOTOR_Typedef MSE_V_MOTOR_LIFT_L;
extern struct MSE_MOTOR_Typedef MSE_V_MOTOR_LIFT_R;
//前伸
extern struct MSE_MOTOR_Typedef MSE_V_MOTOR_HORIZONTAL_L;
extern struct MSE_MOTOR_Typedef MSE_V_MOTOR_HORIZONTAL_R;
//横移
extern struct MSE_MOTOR_Typedef MSE_V_MOTOR_SWAY;
//Yaw轴
extern struct MSE_MOTOR_Typedef MSE_V_MOTOR_Yaw;
extern struct MSE_MOTOR_Typedef MSE_V_MOTOR_L_CUP_PUMP;
extern struct MSE_MOTOR_Typedef MSE_V_MOTOR_R_CUP_PUMP;
//妙达4310
extern struct MSE_MOTOR_Typedef MSE_V_MOTOR_Pitch;

extern struct MSE_MOTOR_Typedef MSE_V_MOTOR_Camera;

extern struct MSE_MOTOR_Typedef MSE_V_MOTOR_BOTTOM_3508_1;
extern struct MSE_MOTOR_Typedef MSE_V_MOTOR_BOTTOM_3508_2;
extern struct MSE_MOTOR_Typedef MSE_V_MOTOR_BOTTOM_3508_3;
extern struct MSE_MOTOR_Typedef MSE_V_MOTOR_BOTTOM_3508_4;

//电机解算函数
void MSE_F_MOTOR_CAN_RX_3508RM_WITHOUT_ANGLE(struct MSE_MOTOR_Typedef* MOTOR , uint8_t* can_data);
void WHW_F_MOTOR_CAN_RX_6020RM(struct MSE_MOTOR_Typedef* MOTOR , uint8_t* can_data);
void MSE_F_MOTOR_CAN_RX_3508RM(struct MSE_MOTOR_Typedef* MOTOR , uint8_t* can_data);
void MSE_F_MOTOR_CAN_RX_2006RM(struct MSE_MOTOR_Typedef* MOTOR , uint8_t* can_data);

//头部电机清空函数
void MSE_F_HEAD_MOTOR_CLEAR(struct MSE_MOTOR_Typedef* MOTOR);
//头部电机重置函数
void WHW_F_HEAD_MOTOR_SIT(struct MSE_MOTOR_Typedef* MOTOR);

//电机堵转检测函数
void MSE_F_HEAD_MOTOR2006_STUCK(struct MSE_MOTOR_Typedef* MOTOR , uint16_t ERROR_ANGLE , uint16_t ERROR_SPEED , uint16_t ERROR_TIME);
void MSE_F_HEAD_MOTOR6020_STUCK(struct MSE_MOTOR_Typedef* MOTOR , uint16_t ERROR_ANGLE , uint16_t ERROR_SPEED , uint16_t ERROR_TIME);
void MSE_F_HEAD_MOTOR3508_STUCK(struct MSE_MOTOR_Typedef* MOTOR , uint16_t ERROR_CURRENT , uint16_t ERROR_SPEED);
void YAW_MOTOR_HOME();


#endif
